#include "motor.h"
#include "gpio.h"
#include "pwm.h"

// 编码器转占空比(0~100)比例系数
#define PRO_CO       (100.0f  / 71.0f)
// 占空比比例换算系数
#define DUTY_CO      (1023.0f / 100.0f)              // 选择PWM_Bit_10

// 电机初始化
void Motor_Init(void)
{
    // 左电机(A)初始化
    GPIO_Init(GPIO_PIN_26,OUTPUT);                   // AIN1
    GPIO_Init(GPIO_PIN_25,OUTPUT);                   // AIN2
    PWM_Init(GPIO_PIN_33,PWM_CH_0,1e4,PWM_Bit_10);   // PWMA

    // 右电机(B)初始化
    GPIO_Init(GPIO_PIN_27,OUTPUT);                   // BIN1
    GPIO_Init(GPIO_PIN_14,OUTPUT);                   // BIN2
    PWM_Init(GPIO_PIN_12,PWM_CH_1,1e4,PWM_Bit_10);   // PWMB

    //初始化电机的转速
	Motor_Set(Motor_Left,  0);
	Motor_Set(Motor_Right, 0);
}

// 还有待修改，根据实际的情况来选择相对的正反转
// 选择电机并且选择对应的速度，-71 ~ +71                 // 5V电压(10ms)下测出来的数据
// 选择电机并且选择对应的速度，-215 ~ +215               // 5V电压(30ms)下测出来的数据
// 选择电机并且选择对应的速度，-210~ +210                // 12V电压(30ms)下测出来的数据
// 输入0~1023
void Motor_Set(MOTOR MotorSign,int16_t Velocity)
{
    // 后续再加入一个数据限幅度// 用宏定义
    // 左电机控制
    if(MotorSign == Motor_Left)
    {
        //正转
		if(Velocity > 0)
		{
			GPIO_WritePin(GPIO_PIN_26,LOW);
			GPIO_WritePin(GPIO_PIN_25,HIGH);
			PWM_SetDuty(PWM_CH_0, Velocity);
		}
		//反转
		else if(Velocity < 0)
		{
			GPIO_WritePin(GPIO_PIN_26,HIGH);
			GPIO_WritePin(GPIO_PIN_25,LOW);
			PWM_SetDuty(PWM_CH_0, -Velocity);
		}
		//制动
		else
		{
            GPIO_WritePin(GPIO_PIN_26,LOW);
			GPIO_WritePin(GPIO_PIN_25,LOW);
			PWM_SetDuty(PWM_CH_0, 0);
		}
    }

    // 右电机控制
    if(MotorSign == Motor_Right)
    {
        //正转
		if(Velocity > 0)
		{
			GPIO_WritePin(GPIO_PIN_27,LOW);
			GPIO_WritePin(GPIO_PIN_14,HIGH);
			PWM_SetDuty(PWM_CH_1, Velocity);
		}
		//反转
		else if(Velocity < 0)
		{
			GPIO_WritePin(GPIO_PIN_27,HIGH);
			GPIO_WritePin(GPIO_PIN_14,LOW);
			PWM_SetDuty(PWM_CH_1, -Velocity);
		}
		//制动
		else
		{
            GPIO_WritePin(GPIO_PIN_27,LOW);
			GPIO_WritePin(GPIO_PIN_14,LOW);
			PWM_SetDuty(PWM_CH_1, 0);
		}
    }
}